#include <stddef.h>
#include <unistd.h>

#include "system.h"
#include "altera_avalon_pio_regs.h"
#include "alt_types.h"

#include "7seg.h"
#include "core.h"
#include "globals.h"
#include "keypad.h"
#include "lcd.h"
#include "pwms.h"
#include "recording.h"

/* Movement operations */
void MoveBaseLeft(void)
{     
    if (RecordingMode())
    {
        RecordEvent(EVENT_ROTATE_LEFT, 1);
    }
    
    Write7Seg("0001");
    WriteLCD("<- BASE LEFT\n<-");
    DecPWM(ROTATEPWM_BASE, PWM_INC);
    return;
}

void MoveBaseRight(void)
{
    if (RecordingMode())
    {
        RecordEvent(EVENT_ROTATE_RIGHT, 1);
    }
    
    Write7Seg("0002");
    WriteLCD("-> BASE RIGHT\n->");
    IncPWM(ROTATEPWM_BASE, PWM_INC);
    return;
}

void MoveShoulderUp(void)
{
    if (RecordingMode())
    {
        RecordEvent(EVENT_SHOULDER_UP, 1);
    }
    
    Write7Seg("0003");
    WriteLCD("^^ SHOULDER\n||    UP");
    IncPWM(SHOULDERPWM_BASE, PWM_INC);
    return;
}

void MoveShoulderDown(void)
{
    if (RecordingMode())
    {
        RecordEvent(EVENT_SHOULDER_DOWN, 1);
    }
    
    Write7Seg("0004");
    WriteLCD("|| SHOULDER\nvv   DOWN");
    DecPWM(SHOULDERPWM_BASE, PWM_INC);
    return;
}

void MoveElbowUp(void)
{
    if (RecordingMode())
    {
        RecordEvent(EVENT_ELBOW_UP, 1);
    }
    
    Write7Seg("0005");
    WriteLCD("^^ ELBOW UP\n||");
    IncPWM(ELBOWPWM_BASE, PWM_INC);
    return;
}

void MoveElbowDown(void)
{
    if (RecordingMode())
    {
        RecordEvent(EVENT_ELBOW_DOWN, 1);
    }
    
    Write7Seg("0006");
    WriteLCD("|| ELBOW DOWN\nvv");
    DecPWM(ELBOWPWM_BASE, PWM_INC);
    return;
}

void InvertGripper(void)
{ 
    if (RecordingMode())
    {
        RecordEvent(EVENT_GRIPPER_INVERT, 1);
    }
    
    InvertGripperPWM();
    Write7Seg("0007");
    usleep(1000000);
    return;
}

void Home(void)
{
    if (RecordingMode())
    {
        RecordEvent(EVENT_HOME, 1);
    }
    
    Write7Seg("0008");
    WriteLCD("/\\ HOME\n||");
    HomePWMs();
    return;
}

void Test(void)
{
    if (RecordingMode())
    {
        RecordEvent(EVENT_TEST, 1);
    }
    
    Write7Seg("TEST");
    TestPWMs();
    return;
}
void CoreWait(void)
{
    usleep(1000);
}
